import re
import matplotlib.pyplot as plt

def parse_gcode(gcode):
    """
    解析 G-code 并提取路径坐标
    """
    paths = []  # 存储每条路径的点
    current_path = []  # 当前路径
    z_level = None  # 当前 Z 轴高度
    travel_height = 5  # 假定的安全移动高度
    
    for line in gcode.splitlines():
        line = line.strip()
        if not line or line.startswith(";"):
            continue
        
        # 匹配 G0/G1 命令中的坐标
        match = re.search(r"(G0|G1)\s+(X[-+]?[0-9]*\.?[0-9]*)?\s*(Y[-+]?[0-9]*\.?[0-9]*)?\s*(Z[-+]?[0-9]*\.?[0-9]*)?", line)
        if match:
            command = match.group(1)
            x = float(match.group(2)[1:]) if match.group(2) else None
            y = float(match.group(3)[1:]) if match.group(3) else None
            z = float(match.group(4)[1:]) if match.group(4) else None
            
            if z is not None:
                z_level = z  # 更新当前 Z 轴高度
                
            # 如果是 G1 且 Z 轴不在安全高度，则记录路径点
            if command == "G1" and z_level is not None and z_level < travel_height:
                current_path.append((x, y))
            
            # 如果是 G0 或路径结束，保存当前路径
            elif command == "G0":
                if current_path:
                    paths.append(current_path)
                    current_path = []
    
    # 保存最后一条路径
    if current_path:
        paths.append(current_path)
    
    return paths

def visualize_paths(paths):
    """
    可视化 G-code 的运动路径
    """
    plt.figure(figsize=(10, 10))
    for path in paths:
        x_coords = [point[0] for point in path]
        y_coords = [point[1] for point in path]
        plt.plot(x_coords, y_coords, marker="o", label="Path")
    
    plt.xlabel("X (mm)")
    plt.ylabel("Y (mm)")
    plt.title("G-code Motion Paths")
    plt.legend()
    plt.grid(True)
    plt.axis("equal")
    plt.show()

# 从文件中读取 G-code 数据
file_path = "./src/utils/output2.gcode"  # 替换为你的文件路径
with open(file_path, "r") as file:
    gcode_data = file.read()

# 解析并可视化
paths = parse_gcode(gcode_data)
visualize_paths(paths)
